/*
 * To use Motor Control Module ,
 * you must initialize the wiringPi library before calling motor_ctrl.
 *
 */

#include "motor_ctrl.h"
#include "../config.h"

int motor_ctrl(unsigned int Direction, unsigned int Length) {

	int result = 0;

#ifdef STEP_MOTOR

	switch (Direction) {
		case MOTOR_UP:
			{
				break;
			};
		case MOTOR_DOWN:
			{
				break;
			};
		case MOTOR_LEFT:
			{
				break;
			};
		case MOTOR_RIGHT:
			{
				break;
			};
	}

#else

	switch (Direction) {
		case MOTOR_UP:
			{
				digitalWrite(DIRA, 1);
				digitalWrite(DIRB, 1);
				digitalWrite(PWMA, 1);
				digitalWrite(PWMB, 1);
				usleep(Length*1000);
				digitalWrite(PWMA, 0);
				digitalWrite(PWMB, 0);
				printf("UP %d ", Length);
				break;
			};
		case MOTOR_DOWN:
			{
				digitalWrite(DIRA, 0);
				digitalWrite(DIRB, 0);
				digitalWrite(PWMA, 1);
				digitalWrite(PWMB, 1);
				usleep(Length*1000);
				digitalWrite(PWMA, 0);
				digitalWrite(PWMB, 0);
				printf("DOWN %d ", Length);
				break;
			};
		case MOTOR_LEFT:
			{
				digitalWrite(DIRA, 1);
				digitalWrite(DIRB, 0);
				digitalWrite(PWMA, 1);
				digitalWrite(PWMB, 1);
				usleep(Length*1000);
				digitalWrite(PWMA, 0);
				digitalWrite(PWMB, 0);
				printf("LEFT %d ", Length);
				break;
			};
		case MOTOR_RIGHT:
			{
				digitalWrite(DIRA, 0);
				digitalWrite(DIRB, 1);
				digitalWrite(PWMA, 1);
				digitalWrite(PWMB, 1);
				usleep(Length*1000);
				digitalWrite(PWMA, 0);
				digitalWrite(PWMB, 0);
				printf("RIGHT %d ", Length);
				break;
			};
		default:
			{
				result = -1;
			}
	}

#endif

	return result;
}
